#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <math.h>
#include <cv.h>
#include <highgui.h>


using namespace cv;
















/** Odległość  **/
inline double distance(Point p1, Point p2)
{
  return sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y));
}





/** Przetwarza kolorowy obraz na wynikowy **/
void reprocess(const char* src_path)
{


  /** Parametry **/
  const int CannyThresh=20;
  const int BinThresh=80;
  const int ratio = 20;
  const int kernel_size = 3;



  /// Load an image
  Mat src = imread( src_path );
  if( !src.data ) return;

  Mat dst,src_gray;
  // konwertuj obraz src do skali szarości src_gray
  cvtColor( src, src_gray, CV_BGR2GRAY );
  // stwórz macierz dst tego samego typu i rozmiaru co src
  dst.create( src_gray.size(), src_gray.type() );
  // redukcja szumów z jądrem (kernel_size x kernel_size)
  blur( src_gray, src_gray, Size(kernel_size,kernel_size) );
  Mat edges;
  // zapisuje edges z obrazu src_gray wykorzystując algorytm Canny
  // CannyThresh- próg 1, CannyThresh*ratio - próg 2 
  // kernel_size- rozmiar macierzy dla operatora Sobela
  Canny( src_gray, edges, CannyThresh, CannyThresh*ratio, kernel_size );
  
  //znajdowanie konturów w edges
  vector<vector<Point> > contours;
  findContours( edges, contours, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
  //binaryzacja edges
  Mat bin_edges;
  threshold( src_gray, bin_edges, BinThresh, 255, CV_THRESH_BINARY );
  
  //Znajdowanie konturow w bin_edges
  vector<vector<Point> > bin_contours;
  findContours( bin_edges, bin_contours, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
  
   //Szukanie najdluzszego konturu	 	
  vector<Point> max=bin_contours[0];
  for(int i=1; i < bin_contours.size(); i++)
    if (bin_contours[i].size()>max.size()) 
      max=bin_contours[i];   
  
  //tylko najdluzszy kontur (obrys ręki)
  contours.clear();
  contours.push_back(max);    
  dst = Scalar::all(0);
  drawContours( dst, contours, CV_FILLED, (255,255,255), 1 );
  
  //  wyznaczanie wrist_reference
  Point wrist_reference=contours[0][0];
  int wrist_reference_iterator=0;
  for(int i=1;i<contours[0].size();++i)
  {
    if(wrist_reference.y>contours[0][i].y)continue;
    wrist_reference=contours[0][i];
    wrist_reference_iterator=i;

  }


  Vector<Point> dists; // x- indeks punktu w contours, y- odleglosc od wrist_reference


  
  // Do dist zapisuję punkty z contours oraz odleglosci zgodnie z ruchem wskazówek zegara zaczynajac od wrist_reference 
  // pomijajac punkty ktorych odleglosc od wrist_reference jest mniejsza od dist_min
  for(int i=0;i<=wrist_reference_iterator;++i)
    dists.push_back(Point(wrist_reference_iterator-i,distance(wrist_reference,contours[0][wrist_reference_iterator-i])));
  for(int i=contours[0].size()-1;i>wrist_reference_iterator;--i)
    dists.push_back(Point(i,distance(wrist_reference,contours[0][i])));

    


  //szukanie maksimów
  const int little_th=120;
  const int ring_th=280;
  const int middle_th=440;
  const int index_th=640;
  const int thumb_th=800;

  const int dist_min=350;

  int little=0,ring=0,middle=0,index=0,thumb=0;

  for(int i=0;i<dists.size();++i)
  {
      if(dists[i].y<dist_min || contours[0][dists[i].x].x>little_th  )continue;
      if(dists[i].y>dists[little].y) little=i;
  }


  for(int i=little;i<dists.size();++i)
  {
      if(dists[i].y<dist_min || contours[0][dists[i].x].x<little_th || contours[0][dists[i].x].x>ring_th)continue;
      if(dists[i].y>dists[ring].y) ring=i;
  }

  for(int i=ring;i<dists.size();++i)
  {
      if(dists[i].y<dist_min || contours[0][dists[i].x].x<ring_th || contours[0][dists[i].x].x>middle_th)continue;
      if(dists[i].y>dists[middle].y) middle=i;
  }

  for(int i=middle;i<dists.size();++i)
  {
      if(dists[i].y<dist_min || contours[0][dists[i].x].x<middle_th || contours[0][dists[i].x].x>index_th)continue;
      if(dists[i].y>dists[index].y) index=i;
  }

  for(int i=index;i<dists.size();++i)
  {
      if(contours[0][dists[i].x].x<index_th || contours[0][dists[i].x].x>thumb_th)continue;
      if(dists[i].y>dists[thumb].y) thumb=i;

  }

  //szukanie minimów

  int little_v=little,ring_v=ring,middle_v=middle,index_v=index;

  for(int i=little;i<ring;++i)
  {
    if(dists[i].y<dist_min)continue;
    if(dists[i].y<dists[little_v].y) little_v=i;
  }
  for(int i=ring;i<middle;++i)
  {
    if(dists[i].y<dist_min)continue;
    if(dists[i].y<dists[ring_v].y) ring_v=i;
  }
  for(int i=middle;i<index;++i)
  {
    if(dists[i].y<dist_min)continue;
    if(dists[i].y<dists[middle_v].y) middle_v=i;
  }
  for(int i=index;i<thumb;++i)
    if(dists[i].y<dists[index_v].y) index_v=i;

  
  //zaznaczenie wrist_reference i first wrist na obrazie;
  int radius=10;
  Point p,q,wr=wrist_reference;
  wr.x+=radius;
  wr.y-=radius;
  circle(dst, wr, radius, (255,255,255), -1);

  //zaznaczenie koniuszków palców
  p=contours[0][dists[little].x];
  p.x+=radius/2;
  p.y+=radius/2;
  circle(dst, p, radius, (255,255,255), -1);
  p=contours[0][dists[ring].x];
  p.x+=radius/2;
  p.y+=radius/2;
  circle(dst, p, radius, (255,255,255), -1);
  p=contours[0][dists[middle].x];
  p.x+=radius/2;
  p.y+=radius/2;
  circle(dst, p, radius, (255,255,255), -1);
  p=contours[0][dists[index].x];
  p.x+=radius/2;
  p.y+=radius/2;
  circle(dst, p, radius, (255,255,255), -1);
  p=contours[0][dists[thumb].x];
  p.x+=radius/2;
  p.y+=radius/2;
  circle(dst, p, radius, (255,255,255), -1);
  //zaznaczenie dolinek i linii
  p=contours[0][dists[little_v].x];
  p.x+=radius/2;
  p.y+=radius/2;
  circle(dst, p, radius, (255,255,255), -1);

  q=contours[0][dists[ring_v].x];
  q.x+=radius/2;
  q.y+=radius/2;
  circle(dst, q, radius, (255,255,255), -1);
  line(dst,p ,q,(255,255,255), 1, 8, 0);


  p=contours[0][dists[middle_v].x];
  p.x+=radius/2;
  p.y+=radius/2;
  circle(dst, p, radius, (255,255,255), -1);
  line(dst,q ,p,(255,255,255), 1, 8, 0);

  q=contours[0][dists[index_v].x];
  q.x+=radius/2;
  q.y+=radius/2;
  circle(dst, q, radius, (255,255,255), -1);
  line(dst,p ,q,(255,255,255), 1, 8, 0);


  line(dst,q ,wr,(255,255,255), 1, 8, 0);

  imwrite(src_path+"_processed", dst);

}


/** @function main */
int main( int argc, char** argv )
{
  if (argc < 2)return -1;
  reprocess(argv[1],"./out.jpeg");
  return 0;
}
 
